import numpy as np
import pytest

from adcs import a_io, omega_mat, unit_vector

# Satellite position and satellite velocity to check the matrix
r = np.array([1e7, 2e7, 3e7])
v = np.array([6e2, 5e2, 4e2])

# Maximum relative error
MAX_RELATIVE_ERROR = 0.01

unit_vector_test_data = [
    pytest.param([10, 0, 0], [1, 0, 0], id="big positive x"),
    pytest.param([0, -0.2, 0], [0, -1, 0], id="small negative y"),
    pytest.param([2, 2, 1], [2 / 3, 2 / 3, 1 / 3], id="all positive big"),
    pytest.param([-0.1, -0.2, -0.2], [-1 / 3, -2 / 3, -2 / 3], id="all negative small"),
    pytest.param([2, -1, 2], [2 / 3, -1 / 3, 2 / 3], id="positive and negative big"),
]

a_io_test_data = [
    pytest.param(np.linalg.det(a_io(r, v)), 1, id="det(M)=1"),
    pytest.param(np.dot(a_io(r, v), a_io(r, v).T), np.eye(3), id="M*M_T=I"),
    pytest.param(np.dot(a_io(r, v).T, a_io(r, v)), np.eye(3), id="M_T*M=I"),
    pytest.param(
        a_io([0, 0, -1], [1, 0, 0]).T,
        np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
        id="r,v parallel to axes of inertial frame case #1",
    ),
    pytest.param(
        a_io([1, 0, 0], [0, 0, 1]).T,
        np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]),
        id="r,v parallel to axes of inertial frame case #2",
    ),
    pytest.param(
        a_io([0, 0, 1], [-1, 0, 0]).T,
        np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]]),
        id="r,v parallel to axes of inertial frame case #3",
    ),
    pytest.param(
        a_io([-1, 0, 0], [0, 0, -1]).T,
        np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]]),
        id="r,v parallel to axes of inertial frame case #4",
    ),
]

omega_mat_test_data = [
    pytest.param([1e2, 2e2, 3e2], id="Omega vector all components positive"),
    pytest.param([-1e2, -2e2, -3e2], id="Omega vector all components negative"),
    pytest.param([1, 0, 0], id="Omega vector only x nonzero"),
    pytest.param([0, 5, 0], id="Omega vector only y nonzero"),
    pytest.param([0, 0, -10], id="Omega vector only z nonzero"),
]


@pytest.mark.parametrize("vector, expected_unit_vector", unit_vector_test_data)
def test_unit_vector(vector, expected_unit_vector):
    # все компоненты посчитанного единичного вектора должны совпадать с соответствующими
    # компонентами ожидаемого единичного вектора
    assert (unit_vector(vector) == np.array(expected_unit_vector)).all()


@pytest.mark.parametrize("matrix, expected_matrix", a_io_test_data)
def test_a_io(matrix, expected_matrix):
    assert np.allclose(matrix, expected_matrix, rtol=MAX_RELATIVE_ERROR)


@pytest.mark.parametrize("omega_vec", omega_mat_test_data)
def test_omega_mat(omega_vec):
    mat = np.array(omega_mat(omega_vec))

    # All zeros on diagonal
    assert np.allclose(np.diag(mat), np.zeros(4), rtol=MAX_RELATIVE_ERROR)

    # A.T=-A (skew - symmetric matrix)
    assert np.allclose(mat.T, -mat, rtol=MAX_RELATIVE_ERROR)
